#include "uwbautofollow/obstacle_grid.h"

#define RAD2DEG(x) ((x) * 180. / M_PI)
#define DEG2RAD(x) ((x) * M_PI / 180.)

ObstacleGrid::ObstacleGrid()
{
    clear_obstacle();
}
ObstacleGrid::~ObstacleGrid()
{
}

void ObstacleGrid::clear_obstacle()
{
    for (int i = 0; i < GRID_NUM; i++)
    {
        for (int j = 0; j < GRID_NUM; j++)
        {
            obstacle_raw[i][j] = 0;
            obstacle_inflation[i][j] = 0;
        }
    }

    for (int i = 0; i < GRID_ANGLE_LEN; i++)
    {
        obs_angle_inflation[i] = 0;
    }
}

void ObstacleGrid::process_data_angle(const sensor_msgs::LaserScan::ConstPtr &msg)
{
    int count = msg->ranges.size();
    float degree;

    for (int i = 0; i < count; i++)
    {
        degree = (msg->angle_min + msg->angle_increment * i);
        if (degree >= 0)
        {
            degree = degree - M_PI; // right: 0 ~ -M_PI/2
        }
        else
        {
            degree = degree + M_PI; // left: 0 ~ M_PI/2
        }

        if (-M_PI / 2 <= degree && degree <= M_PI / 2 && 0.1 < msg->ranges[i] && msg->ranges[i] < GRID_DIS_MAX / 100.0)
        {
            degree = RAD2DEG(degree);
            int idx = DEGREE_TO_IDX(degree);

            // ROS_INFO("Lindar,angle = %f, idx = %d:", degree, idx);

            string debug_str = "inflation:" + std::to_string(degree);

            if (idx >= 0 && idx < GRID_ANGLE_LEN && (obs_angle_inflation[idx] == 0 || msg->ranges[i] < obs_angle_inflation[idx]))
            {

                obs_angle_inflation[idx] = msg->ranges[i];

                int start = 0;
                int end = GRID_ANGLE_LEN - 1;
                if (INFLATION_RADIUS < msg->ranges[i])
                {
                    float tmp_angle = RAD2DEG(asin(INFLATION_RADIUS / msg->ranges[i]));

                    debug_str += "(" + std::to_string(tmp_angle) + ")";

                    start = DEGREE_TO_IDX(degree - tmp_angle);
                    end = DEGREE_TO_IDX(degree + tmp_angle);
                }
                else
                {
                    start = DEGREE_TO_IDX(degree - 90);
                    end = DEGREE_TO_IDX(degree + 90);
                }
                if (start < 0)
                    start = 0;
                if (end > GRID_ANGLE_LEN - 1)
                    end = GRID_ANGLE_LEN - 1;
                for (int j = start; j <= end; j++)
                {
                    if (j >= 0 && j < GRID_ANGLE_LEN)
                    {
                        if (obs_angle_inflation[j] == 0 || msg->ranges[i] < obs_angle_inflation[j])
                        {
                            obs_angle_inflation[j] = msg->ranges[i];
                        }
                    }
                }

                debug_str += "(" + std::to_string(start) + ", " + std::to_string(end) + ")";
                // ROS_INFO("debug_str: %s", debug_str.c_str());
            }
        }
    }
}

void ObstacleGrid::process_data(const sensor_msgs::LaserScan::ConstPtr &msg)
{
    clear_obstacle();

    process_data_angle(msg);

    process_data_grid(msg);
}
void ObstacleGrid::process_data_grid(const sensor_msgs::LaserScan::ConstPtr &msg)
{
    int count = msg->ranges.size();
    int x;
    int y;
    float degree;

    for (int i = 0; i < count; i++)
    {
        degree = (msg->angle_min + msg->angle_increment * i);
        if (degree >= 0)
        {
            degree = degree - M_PI; // right: 0 ~ -M_PI/2
        }
        else
        {
            degree = degree + M_PI; // left: 0 ~ M_PI/2
        }

        if (-M_PI / 2 <= degree && degree <= M_PI / 2 && 0.1 < msg->ranges[i] && msg->ranges[i] < GRID_DIS_MAX / 100.0)
        {
            x = msg->ranges[i] * sin(degree) * 100 / GRID_DIS + GRID_NUM / 2;
            y = msg->ranges[i] * cos(degree) * 100 / GRID_DIS;

            if (x >= 0 && x < GRID_NUM && y >= 0 && y < GRID_NUM)
            {
                obstacle_raw[x][y]++;

                // if (msg->ranges[i] <= 0.6 + 0.2)
                //     ROS_INFO("obstacles_raw: %f, %f, %d, %d:", degree, msg->ranges[i], x, y);
            }
        }
    }
}

// width,height: meter
//      ------------------   y=GRID_NUM
//     |                  |
//     |                  |  height
//     |                  |
//  x=GRID_NUM          x=0, y=0
//  left       width      right
bool ObstacleGrid::detect_obstacle(float width, float height)
{
    int x_start, x_end, y_start, y_end;
    x_start = GRID_NUM / 2 - width / 2 * 100 / GRID_DIS;
    x_end = GRID_NUM / 2 + width / 2 * 100 / GRID_DIS;
    y_start = 0;
    y_end = height * 100 / GRID_DIS;

    // ROS_INFO("detect_obstacle begin. %d %d %d %d", x_start, x_end, y_start, y_end);

    if (x_start < 0)
        x_start = 0;
    if (x_end > GRID_NUM - 1)
        x_end = GRID_NUM - 1;
    if (y_end > GRID_NUM - 1)
        y_end = GRID_NUM - 1;

    for (int i = x_start; i <= x_end; i++)
    {
        for (int j = y_start; j <= y_end; j++)
        {
            if (obstacle_raw[i][j] > 0)
            {
                ROS_INFO("detect_obstacle,i,j = %d, %d", i, j);

                return true;
            }
        }
    }
    return false;
}

// target_angle left:positive; right: negtive
// return: (degree), -180 means finding no way.
float ObstacleGrid::find_anlge_of_way(float target_angle, float target_distance, float safe_distance)
{

    float turn_angle = -180;
    int idx_target = DEGREE_TO_IDX(target_angle);
    if (idx_target < 0 || idx_target >= GRID_ANGLE_LEN)
    {
        deadzone_turn_flag = 0;
        return -180; // not found
    }
    if (obs_angle_inflation[idx_target] == 0 || obs_angle_inflation[idx_target] > safe_distance)
    {
        turn_angle = target_angle;
    }
    else
    {
        int idx_left = -1;
        for (int i = idx_target + 1; i < GRID_ANGLE_LEN; i++)
        {
            if (obs_angle_inflation[i] == 0 || obs_angle_inflation[i] > safe_distance)
            {
                ROS_INFO("find way,left =%d, %d", idx_target, i);
                idx_left = i;
                break;
            }
        }

        int idx_right = -1;
        for (int i = idx_target - 1; i >= 0; i--)
        {
            if (obs_angle_inflation[i] == 0 || obs_angle_inflation[i] > safe_distance)
            {
                ROS_INFO("find way,right =%d, %d", idx_target, i);
                idx_right = i;
                break;
            }
        }

        if (idx_left >= 0 && idx_right >= 0)
        {
            float leftAngle = IDX_TO_DEGREE(idx_left);
            float rightAngle = IDX_TO_DEGREE(idx_right);

            if (abs(rightAngle - target_angle) > 90 && abs(leftAngle - target_angle) < 90)
            {
                turn_angle = leftAngle;
            }
            else if (abs(leftAngle - target_angle) > 90 && abs(rightAngle - target_angle) < 90)
            {
                turn_angle = rightAngle;
            }
            else
            {
                if (deadzone_turn_flag > 0 && leftAngle * rightAngle < 0)
                {
                    if (deadzone_turn_flag == 1)
                    { // turn left

                        if (abs(rightAngle) + 5 <= abs(leftAngle))
                        {
                            turn_angle = rightAngle;
                        }
                        else
                        {
                            turn_angle = leftAngle;
                        }
                    }
                    else if (deadzone_turn_flag == 2)
                    { // turn right
                        if (abs(leftAngle) + 5 <= abs(rightAngle))
                        {
                            turn_angle = leftAngle;
                        }
                        else
                        {
                            turn_angle = rightAngle;
                        }
                    }
                }
                else
                {
                    if (abs(leftAngle) <= abs(rightAngle))
                    {
                        turn_angle = IDX_TO_DEGREE(idx_left);
                    }
                    else
                    {
                        turn_angle = IDX_TO_DEGREE(idx_right);
                    }
                }
            }

            // if (abs(idx_target - idx_left) <= abs(idx_target - idx_right))
            // if (abs(idx_left - DEGREE_TO_IDX(0)) + abs(idx_target - idx_left) <= abs(idx_right - DEGREE_TO_IDX(0)) + abs(idx_target - idx_right))
        }
        else if (idx_left >= 0)
        {
            float leftAngle = IDX_TO_DEGREE(idx_left);
            turn_angle = leftAngle;
        }
        else if (idx_right >= 0)
        {
            float rightAngle = IDX_TO_DEGREE(idx_right);
            turn_angle = rightAngle;
        }
    }


    if (turn_angle == -180)
    {
        deadzone_turn_flag = 0; // not found
    }
    else if (turn_angle > 5)
        deadzone_turn_flag = 1;
    else if (turn_angle < -5)
        deadzone_turn_flag = 2;
    else
        deadzone_turn_flag = 0;

    return turn_angle;
}
